// ============================================================================
// Core 2
// 
// Description: 
// Splits the job into multiple smaller tasks and assigns them to either core 0
// or core 1 by placing them in a queue. Results of processed tasks are 
// collected from the queue and are written back to frame buffer.
// 
// ============================================================================

#include "memmap.h"
#include "compsoc.h"

#include "../filter_ex4b_common/structures.h"
#include "../filter_ex4b_common/filter.h"
#include "../filter_ex4b_common/energy.h"

#define max(a,b) (((a) > (b)) ? (a) : (b))
#define min(a,b) (((a) < (b)) ? (a) : (b))

#define SOURCEIMAGE (2)

// ============================================================================
// Data mapping
// ============================================================================

// Use the global memory as the task queue
#define taskQueue ((Queue*) shared_mem_pt)

// Use the frame buffer as the source image
#define sourceImage ((Image*) frame_mem_pt)

// Use rdma0 as a temporary slot
DMA dma0 = {1, (void*) dma0_BASEADDR};
#define tempSlot ((Slot*) cmemout0_BASEADDR)

// Use tile 0 and 1 synchronization registers as queue states
#define stateTileQueue0 ((TileQueueState*) mb0_cmemin0_pt)
#define stateTileQueue1 ((TileQueueState*) mb1_cmemin0_pt)

// Use rdma0 as a temporary slot state
#define stateTemp ((SlotState*) cmemout0_BASEADDR) 

// Use synchronization register as queue state
#define stateQueue ((QueueState*) cmemin0_BASEADDR) 

// Use Dmem as a local buffer
Row localBuffer[7];

// ============================================================================
// Transfer functions
// ============================================================================

void copyRows(Row* source, Row* destination, int numberOfRows) {

	memcpy(destination, source, sizeof(Row) * numberOfRows);
}

// Reads/writes rows from the source image to the temp slot
void readRowsFromImage(int srcStartRow, int numberOfRows, int destStartRow) {
	
	hw_dma_receive((void*) &tempSlot->rows[destStartRow], (void*) &sourceImage->rows[srcStartRow], (sizeof(Row) * numberOfRows) / 4, &dma0);
}

void writeRowsToImage(int destStartRow, int numberOfRows, int srcStartRow) {

	hw_dma_send((void*) &sourceImage->rows[destStartRow], (void*) &tempSlot->rows[srcStartRow], (sizeof(Row) * numberOfRows) / 4, &dma0);
}

// Read/write slot from the task queue to the temp slot
void readSlotFromQueue(int tile, int slot) {

	hw_dma_receive((void*) tempSlot, (void*) &(taskQueue->tiles[tile].slots[slot]), sizeof(Slot) / 4, &dma0);
}

void writeSlotToQueue(int tile, int slot) {

	hw_dma_send((void*) &taskQueue->tiles[tile].slots[slot], (void*) tempSlot, sizeof(Slot) / 4, &dma0);
}

// Write slot state (remote and local)
void updateQueueSlot(int tile, int slot, unsigned char state, short startRow, unsigned char numRows) {

	stateTemp->state = state;
	stateTemp->startRow = startRow;
	stateTemp->numRows = numRows;

	hw_dma_send((void*) &(tile == 0 ? stateTileQueue0 : stateTileQueue1)->slots[slot], (void*) stateTemp, sizeof(SlotState) / 4, &dma0);

	stateQueue->tiles[tile].slots[slot] = (*stateTemp);
}

// Ensure all writes have been processed
void flushWrites() {

	hw_dma_receive((void*) tempSlot, (void*) sourceImage, 1, &dma0);
}

// ============================================================================
// Globals
// ============================================================================

// State of tiles
#define TILESTATE_WORKING (0)
#define TILESTATE_FINALIZING (1)
#define TILESTATE_DONE (2)

char tileState[3] = {TILESTATE_WORKING, TILESTATE_WORKING, TILESTATE_WORKING};

// Work division between tiles
#define TILE0_SLOTS (21)
#define TILE1_SLOTS (21)

short startNo[3] = {0, TILE0_SLOTS * 5 + 3, (TILE0_SLOTS + TILE1_SLOTS) * 5 + 6};
short endNo[3] = {TILE0_SLOTS * 5 + 2, (TILE0_SLOTS + TILE1_SLOTS) * 5 + 5, 319};

// endNo[0] = 107
// endNo[1] = 215

// ============================================================================
// Queue scheduling
// ============================================================================

int prepareStartTask(int tile, int slot, int lineNo, int startNo, int endNo) {

	if (lineNo == -1) {
		readRowsFromImage(0, 4, 1);
		tempSlot->rows[0] = tempSlot->rows[1];
	} else {
		readRowsFromImage(lineNo, 5, 0);
	}

	writeSlotToQueue(tile, slot);
	updateQueueSlot(tile, slot, STATE_START, lineNo, 5);

	print_debug(tile * 100 + 3);

	return lineNo + 5;
}

int prepareStopTask(int tile, int slot, int lineNo, int startNo, int endNo) {

	readRowsFromImage(endNo - 3, 5, 0);

	writeSlotToQueue(tile, slot);
	updateQueueSlot(tile, slot, STATE_STOP, endNo - 3, 5);

	print_debug(tile * 100 + 4);

	return lineNo;
}

int prepareNextTask(int tile, int slot, int lineNo, int startNo, int endNo) {

	readRowsFromImage(lineNo, 5, 0);
	writeSlotToQueue(tile, slot);
	updateQueueSlot(tile, slot, STATE_UNPROCESSED, lineNo, 5);

	print_debug(tile * 100 + 5);

	if (lineNo == endNo - 8) {
		tileState[tile] = TILESTATE_FINALIZING;
	}

	return lineNo + 5;
}

void processTaskResult(int tile, int slot, SlotState state) {

	readSlotFromQueue(tile, slot);
	
	if (state.startRow + (state.numRows - 1) >= endNo[tile]) {
		writeRowsToImage(endNo[tile] - (state.numRows - 1), state.numRows, 0);
		print_debug(tile * 100 + 7 + (state.startRow + state.numRows) * 10000);
		tileState[tile] = TILESTATE_DONE;
	} else {
		writeRowsToImage(state.startRow, state.numRows, 0);
		print_debug(tile * 100 + 6 + (state.startRow + state.numRows) * 10000);
	}

	stateQueue->tiles[tile].slots[slot].state = STATE_EMPTY;
}

// ============================================================================
// Local filtering
// ============================================================================

int filterNextSlot(int lineNo, int startNo, int endNo) {

	if (lineNo == startNo - 1) {

		readRowsFromImage(lineNo, 5, 0);
	
		copyRows(&tempSlot->rows[0], &localBuffer[2], 5);
	
		filterRow(&localBuffer[2], &localBuffer[3], &localBuffer[4], &tempSlot->rows[2], lineNo + 1);
		filterRow(&localBuffer[3], &localBuffer[4], &localBuffer[5], &tempSlot->rows[3], lineNo + 2);
		filterRow(&localBuffer[4], &localBuffer[5], &localBuffer[6], &tempSlot->rows[4], lineNo + 3);

		writeRowsToImage(lineNo + 1, 3, 2);

		copyRows(&localBuffer[5], &localBuffer[0], 2);

		print_debug(3 * 100 + 1);

	} else if (lineNo == endNo + 1) {
	
		tileState[2] = TILESTATE_DONE;

		filterRow(&localBuffer[5], &localBuffer[6], &localBuffer[6], &tempSlot->rows[0], lineNo - 1);
		writeRowsToImage(lineNo - 1, 1, 0);

		print_debug(3 * 100 + 2);

	} else {

		readRowsFromImage(lineNo, 5, 0);
	
		copyRows(&tempSlot->rows[0], &localBuffer[2], 5);
	
		filterRow(&localBuffer[0], &localBuffer[1], &localBuffer[2], &tempSlot->rows[0], lineNo - 1);
		filterRow(&localBuffer[1], &localBuffer[2], &localBuffer[3], &tempSlot->rows[1], lineNo + 0);
		filterRow(&localBuffer[2], &localBuffer[3], &localBuffer[4], &tempSlot->rows[2], lineNo + 1);
		filterRow(&localBuffer[3], &localBuffer[4], &localBuffer[5], &tempSlot->rows[3], lineNo + 2);
		filterRow(&localBuffer[4], &localBuffer[5], &localBuffer[6], &tempSlot->rows[4], lineNo + 3);

		writeRowsToImage(lineNo - 1, 5, 0);

		copyRows(&localBuffer[5], &localBuffer[0], 2);

		print_debug(3 * 100 + 3);
	}

	return lineNo + 5;
}

// ============================================================================
// Main entry point
// ============================================================================

int main() {

	// Initialize framework
	mon_sync(SOURCEIMAGE);

	print_debug(1);

	// Set most energy efficient frequency
	setOptimalFreq();

	// Current row.
	int lineNo[3] = {startNo[0] - 1, startNo[1] - 1, startNo[2] - 1};

	// Prepare start and stop tasks
	lineNo[0] = prepareStartTask(0, 0, lineNo[0], startNo[0], endNo[0]);
	lineNo[1] = prepareStartTask(1, 0, lineNo[1], startNo[1], endNo[1]);
	
	lineNo[0] = prepareStopTask(0, 1, lineNo[0], startNo[0], endNo[0]);
	lineNo[1] = prepareStopTask(1, 1, lineNo[1], startNo[1], endNo[1]);

	print_debug(2);

	// Start the main processing loop.
	// - if there is an empty slot in the queue of a tile, we prepare a new task for this tile.
	// - if there is a processed slot in the queue, we write the resulting rows to the frame buffer and mark the slot as empty.
	// - if there is no other work to be done, we start filtering rows ourselves.

	while(!(tileState[0] == TILESTATE_DONE && tileState[1] == TILESTATE_DONE && tileState[2] == TILESTATE_DONE)) {
		
		// Loop through each slot in both queues
		int idle = TRUE;

		int tile, slot;

		for (slot = 0; slot < QUEUE_LENGTH; slot++) {
			for (tile = 0; tile < TILE_NUM; tile++) {

				SlotState slotState = stateQueue->tiles[tile].slots[slot];

				if (slotState.state == STATE_EMPTY && tileState[tile] == TILESTATE_WORKING) {
					lineNo[tile] = prepareNextTask(tile, slot, lineNo[tile], startNo[tile], endNo[tile]);
					idle = FALSE;
				} else if (slotState.state == STATE_PROCESSED) {
					processTaskResult(tile, slot, slotState);
					idle = FALSE;
				}
			}
		}

		// If we are idling we can filter own slots.
		if (idle && tileState[2] == TILESTATE_WORKING) {

			lineNo[2] = filterNextSlot(lineNo[2], startNo[2], endNo[2]);
		}
	}

	// Wait until all data has been send to the frame buffers
	flushWrites();

	print_debug(8);

	// Print result
	print_framebuffer();

	return 0;
}

